#include "mainwindow.h"
#include "imageviewer.h"
#include "siftfeaturedetector.h"
#include "featurematcher.h"
#include "ransacimagealignment.h"

#include <stdio.h>
#include <iostream>

#include <QAction.h>
#include <QMenu.h>
#include <QMenuBar.h>
#include <QMessageBox.h>
#include <QLabel.h>
#include <QImage.h>
#include <QPixmap.h>
#include <QPainter.h>
#include <QFileDialog.h>

#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#define COMPANY_NAME "PhotoPano"
#define APP_NAME "PhotoPano"
#define VERSION_INFO "Version 1.0 (Dec 2011)\n     http://code.google.com/p/photo-pano/ \n     grant.grubb@gmail.com"


/******************************************************/
MainWindow::MainWindow():
QMainWindow()
 {
	createActions();
	createMenus();
	setWindowTitle("PhotoPano");

	doSomething();
 }

/******************************************************/
MainWindow::~MainWindow()
{}

/******************************************************/
void MainWindow::doSomething()
{
	// Do some OpenCV tests with jpeg compression
    //cv::Mat img_orig = cv::imread("img.png");
    //std::vector<uchar> buf;
    //std::vector<int> params(2); 
    //params[0] = CV_IMWRITE_JPEG_QUALITY;
    //params[1] = 50;
    //imencode(".jpg", img_orig, buf, params);
    //
    //cv::Mat buf_m(buf);
    //cv::Mat img_decd = cv::imdecode(buf_m, 1);
    //params[0]= CV_IMWRITE_PNG_COMPRESSION;
    //params[1] = 0;
    //cv::imwrite("img_out.png",img_decd, params);

	// Load image in Qt
	QString filename1 = QFileDialog::getOpenFileName(this, tr("Open 1st Image"), ".", tr("Image Files (*.png *.jpg *.bmp)"));
	QString filename2 = QFileDialog::getOpenFileName(this, tr("Open 2nd Image"), ".", tr("Image Files (*.png *.jpg *.bmp)"));
	QImage* qimg1 = new QImage(filename1);
	QImage* qimg2 = new QImage(filename2);
	
	// Draw images
	ImageViewer* results_view1 = new ImageViewer("Image 1 keypoints");
	results_view1->setImage(qimg1);
	
	ImageViewer* results_view2 = new ImageViewer("Image 2 keypoints");
	results_view2->setImage(qimg2);

	// Now try to get into OpenCV format
	cv::Mat img1(qimg1->height(), qimg1->width(), CV_8UC4, qimg1->scanLine(0));
	cv::Mat img2(qimg2->height(), qimg2->width(), CV_8UC4, qimg2->scanLine(0));
	//cv::imshow("Cadel!", img1);

	// Convert image to CV_8UC1
	cv::Mat img1_8uc1, img2_8uc1;
	cv::cvtColor(img1, img1_8uc1, CV_RGB2GRAY, 1);
	cv::cvtColor(img2, img2_8uc1, CV_RGB2GRAY, 1);
	//cv::imshow("Cadel bw!", img1_8uc1);

	// Compute SIFT features
	SiftFeatureDetector sift_detector;
	std::vector<cv::KeyPoint> key_points1, key_points2;
	cv::Mat descriptors1, descriptors2;
	sift_detector.extract(img1_8uc1, key_points1, descriptors1);
	sift_detector.extract(img2_8uc1, key_points2, descriptors2);

	// Display num keypoints in each image
	//QMessageBox::information(this, QString("Debug print"), QString("Num keypoints ") + QString::number(key_points1.size()) + " " + QString::number(key_points2.size()));

	FeatureMatcher feature_matcher;
	std::vector<std::pair<int,int> > correspondances;
	if (key_points1.size() > 0 && key_points2.size() > 0)
	{
		// Find matches between images
		feature_matcher.match(key_points1, key_points2, descriptors1, descriptors2, correspondances);
		
		// Draw resulting correspondences
		QColor colors[6] = {Qt::red, Qt::blue, Qt::white, Qt::yellow, Qt::green, Qt::cyan};
		int color_index = 0;
		for (unsigned int i=0; i < correspondances.size(); ++i)
		{
			QPoint img1_pt(key_points1[correspondances[i].first].pt.x, key_points1[correspondances[i].first].pt.y);
			QPoint img2_pt(key_points2[correspondances[i].second].pt.x, key_points2[correspondances[i].second].pt.y);
			results_view1->drawPoint(img1_pt, 4, colors[color_index]);	
			results_view2->drawPoint(img2_pt, 4, colors[color_index]);	

			color_index++;
			color_index = color_index % 6;
		}
	}

	// Align the images
	std::vector<std::pair<Pt2D, Pt2D> > samples(correspondances.size());
	for (unsigned int i=0; i < correspondances.size(); ++i)
	{
		Pt2D first, second;
		first.x = key_points1[correspondances[i].first].pt.x;
		first.y = key_points1[correspondances[i].first].pt.y;

		second.x = key_points2[correspondances[i].second].pt.x;
		second.y = key_points2[correspondances[i].second].pt.y;

		samples[i] = std::make_pair<Pt2D, Pt2D>(first, second);
	}

	RansacImageAlignment aligner;
	aligner.setSamples(samples);
	aligner.setInlierThreshold(0.6);
	std::vector<double> solution;
	std::vector<int> inliers;
	int num_inliers = aligner.run(solution, inliers);

	if (solution.size() == 9)
	{
		cout << "Solution" << endl;
		for (unsigned int i=0; i < solution.size(); i++)
		{
			cout << solution[i] << endl;
		}
		cout << "Num inliers " << inliers.size() << endl;

		for (unsigned int i=0; i < inliers.size(); ++i)
		{
			QPoint img1_pt(key_points1[correspondances[inliers[i]].first].pt.x, key_points1[correspondances[inliers[i]].first].pt.y);
			QPoint img2_pt(key_points2[correspondances[inliers[i]].second].pt.x, key_points2[correspondances[inliers[i]].second].pt.y);
			results_view1->drawCircle(img1_pt, 2, Qt::black);	
			results_view2->drawCircle(img2_pt, 2, Qt::black);	
		}

		cv::Mat H(3,3,CV_64FC1);
		H.at<double>(0,0) = solution[0];
		H.at<double>(0,1) = solution[1];
		H.at<double>(0,2) = solution[2];
		H.at<double>(1,0) = solution[3];
		H.at<double>(1,1) = solution[4];
		H.at<double>(1,2) = solution[5];
		H.at<double>(2,0) = solution[6];
		H.at<double>(2,1) = solution[7];
		H.at<double>(2,2) = solution[8];

		std::vector<std::pair<cv::Mat, cv::Mat*> > image_set(2); // H and image for entire set

		image_set[0].first = cv::Mat::eye(3,3,CV_64FC1);
		image_set[0].second = &img1_8uc1;
		image_set[1].first = H.inv();
		image_set[1].second = &img2_8uc1;

		double min_x = DBL_MAX;
		double min_y = DBL_MAX;
		double max_x = 0;
		double max_y = 0;

		for (unsigned int im=0; im < image_set.size(); ++im)
		{
			std::vector<cv::Mat> corners(4);
			for (int i=0; i < 4; ++i)
				corners[i] = cv::Mat(3,1, CV_64FC1,1.0);

			corners[0].at<double>(0) = 0.0; corners[0].at<double>(1) = 0.0;
			corners[1].at<double>(0) = image_set[im].second->cols; corners[1].at<double>(1) = 0.0;
			corners[2].at<double>(0) = 0.0; corners[2].at<double>(1) = img2_8uc1.rows;
			corners[3].at<double>(0) = image_set[im].second->cols; corners[3].at<double>(1) = image_set[im].second->rows;
			for (int i=0; i < 4; ++i)
			{
				cv::Mat pt = image_set[im].first * corners[i];
				pt.at<double>(0) /= pt.at<double>(2);
				pt.at<double>(1) /= pt.at<double>(2);
				if (pt.at<double>(0) < min_x)
					min_x = pt.at<double>(0);
				if (pt.at<double>(0) > max_x)
					max_x = pt.at<double>(0);
				if (pt.at<double>(1) < min_y)
					min_y = pt.at<double>(1);
				if (pt.at<double>(1) > max_y)
					max_y = pt.at<double>(1);
			}
		}

		cv::Mat img2_8uc1_trans;
		cv::warpPerspective(img2_8uc1, img2_8uc1_trans, H.inv(), cv::Size(max_x,max_y));

		for (int r=0; r < img1_8uc1.rows; ++r)
		{
			for (int c=0; c < img1_8uc1.cols; ++c)
			{
				if (img2_8uc1_trans.at<unsigned char>(r,c) == 0)
					img2_8uc1_trans.at<unsigned char>(r,c) = img1_8uc1.at<unsigned char>(r,c);
			}
		}
		cv::imshow("transformed", img2_8uc1_trans);
	}
}

/******************************************************/
void MainWindow::createActions()
{
	_exit_act = new QAction(tr("Exit"), this);
	connect(_exit_act, SIGNAL(triggered()), this, SLOT(close()));
}

/******************************************************/
void MainWindow::createMenus()
{
	_file_menu = new QMenu(tr("&File"), this);
	_file_menu->addSeparator();
	_file_menu->addAction(_exit_act);

	menuBar()->addMenu(_file_menu);
}
